/**
* This file is part of LIO-mapping.
* 
* Copyright (C) 2019 Haoyang Ye <hy.ye at connect dot ust dot hk>,
* Robotics and Multiperception Lab (RAM-LAB <https://ram-lab.com>),
* The Hong Kong University of Science and Technology
* 
* For more information please see <https://ram-lab.com/file/hyye/lio-mapping>
* or <https://sites.google.com/view/lio-mapping>.
* If you use this code, please cite the respective publications as
* listed on the above websites.
* 
* LIO-mapping is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
* 
* LIO-mapping is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
* GNU General Public License for more details.
* 
* You should have received a copy of the GNU General Public License
* along with LIO-mapping.  If not, see <http://www.gnu.org/licenses/>.
*/

//
// Created by hyye on 3/27/18.
//

#include <ceres/ceres.h>
#include "imu_processor/Estimator.h"

#include <pcl/common/transforms.h>
#include <pcl/search/impl/flann_search.hpp>
#include <pcl/filters/extract_indices.h>

namespace lio {

#define CHECK_JACOBIAN 0

// WARNING to half actually
size_t TransformToEnd(PointCloudPtr &cloud, Twist<float> transform_es, float time_factor, bool keep_intensity = false) {
  size_t cloud_size = cloud->points.size();

  for (size_t i = 0; i < cloud_size; i++) {
    PointT &point = cloud->points[i];

    float s = time_factor * (point.intensity - int(point.intensity));

//    DLOG(INFO) << "s: " << s;
    if (s < 0 || s > 1 + 1e-3) {
      LOG(ERROR) << "point.intensity: " << point.intensity;
      LOG(ERROR) << "time ratio error: " << s;
    }

    point.x -= s * transform_es.pos.x();
    point.y -= s * transform_es.pos.y();
    point.z -= s * transform_es.pos.z();
    if (!keep_intensity) {
      point.intensity -= int(point.intensity);
    }

    Eigen::Quaternionf q_id, q_s, q_e, q_half;
    q_e = transform_es.rot;
    q_id.setIdentity();
    q_s = q_id.slerp(s, q_e);

    RotatePoint(q_s.conjugate().normalized(), point);

//    q_half = q_id.slerp(0.5, q_e);
//    RotatePoint(q_half, point);
//    point.x += 0.5 * transform_es.pos.x();
//    point.y += 0.5 * transform_es.pos.y();
//    point.z += 0.5 * transform_es.pos.z();
     RotatePoint(q_e, point);

     point.x += transform_es.pos.x();
     point.y += transform_es.pos.y();
     point.z += transform_es.pos.z();
  }

  return cloud_size;
}

void averageTimeDownSmaple(PointCloudPtr input, PointCloudPtr spoint_downsampled_, int step = 3) {
  int n = input->points.size();
  for (size_t idx = 0; idx < n; idx+= step) {
    spoint_downsampled_->points.push_back(input->points[idx]);
  }
}

Estimator::Estimator() {
  ROS_DEBUG(">>>>>>> Estimator started! <<<<<<<");

  ClearState();
}

Estimator::Estimator(EstimatorConfig config, MeasurementManagerConfig mm_config) {
  ROS_DEBUG(">>>>>>> Estimator started! <<<<<<<");

  SetupAllEstimatorConfig(config, mm_config);

  ClearState();
}

Estimator::~Estimator() {

}

void Estimator::SetupAllEstimatorConfig(const EstimatorConfig &config, const MeasurementManagerConfig &mm_config) {

  this->mm_config_ = mm_config;

  if (estimator_config_.window_size != config.window_size) {
    linear_acceleration_buf_.Reset(config.window_size + 1);
    angular_velocity_buf_.Reset(config.window_size + 1);
    imu_timestamp_buf_.Reset(config.window_size + 1);
  }

  down_size_filter_corner_.setLeafSize(config.corner_filter_size, config.corner_filter_size, config.corner_filter_size);
  down_size_filter_surf_.setLeafSize(config.surf_filter_size, config.surf_filter_size, config.surf_filter_size);
  down_size_filter_map_.setLeafSize(config.map_filter_size, config.map_filter_size, config.map_filter_size);
  transform_lb_ = config.transform_lb;

  min_match_sq_dis_ = config.min_match_sq_dis;
  min_plane_dis_ = config.min_plane_dis;
  extrinsic_stage_ = config.estimate_extrinsic;

  estimator_config_ = config;
}

void Estimator::ClearState() {
  // TODO: CirclarBuffer should have clear method

  for (size_t i = 0; i < estimator_config_.window_size + 1;
       ++i) {
    linear_acceleration_buf_[i].clear();
    angular_velocity_buf_[i].clear();
    imu_timestamp_buf_[i].clear();
  }

  stage_flag_ = NOT_INITED;
  first_imu_ = false;
  cir_buf_count_ = 0;

  R_WI_.setIdentity();
  Q_WI_ = R_WI_;

  // WARNING: g_norm should be set before clear
  g_norm_ = 9.805;

  convergence_flag_ = false;

  laserCloudSurfFromMap.reset(new PointCloud());
  laserCloudCornerFromMap.reset(new PointCloud());
  laserCloudSurfFromMapDS.reset(new PointCloud());
  laserCloudCornerFromMapDS.reset(new PointCloud());

  kdtree_surf_from_map.reset(new pcl::KdTreeFLANN<PointT>());
  kdtree_corner_from_map.reset(new pcl::KdTreeFLANN<PointT>());
}

void Estimator::SetupRos(ros::NodeHandle &nh) {
  MeasurementManager::SetupRos(nh);
  PointMapping::SetupRos(nh, false);

  predict_odom_.header.frame_id = "imu_init";
  predict_odom_.child_frame_id = "predict";
  pub_predict_odom_ = nh.advertise<nav_msgs::Odometry>("/predict_laser_odom", 100);

  discrete_imu_odom_.header.frame_id = "imu_init";
  discrete_imu_odom_.child_frame_id = "discrete_imu_predict";
  pub_discrete_imu_odom_ = nh.advertise<nav_msgs::Odometry>("/discrete_imu_odom", 100);

  newest_odom_.header.frame_id = "imu_init";
  newest_odom_.child_frame_id = "newest";
  pub_newest_odom_ = nh.advertise<nav_msgs::Odometry>("/newest_laser_odom", 100);

  past_odom_.header.frame_id = "imu_init";
  past_odom_.child_frame_id = "past";
  pub_past_odom_ = nh.advertise<nav_msgs::Odometry>("/past_laser_odom", 100);

  control_point_.header.frame_id = "imu_init";
  control_point_.child_frame_id = "control";
  pub_control_point_ = nh.advertise<nav_msgs::Odometry>("/newest_control_point", 100);
  pub_opt_cp_ = nh.advertise<nav_msgs::Odometry>("/optimized_control_point", 100);

  pub_local_surf_points_ = nh.advertise<sensor_msgs::PointCloud2>("/local/surf_points", 2);
  pub_local_corner_points_ = nh.advertise<sensor_msgs::PointCloud2>("/local/corner_points", 2);

  pub_past_surf_points_ = nh.advertise<sensor_msgs::PointCloud2>("/past/surf_points", 2);
  pub_past_corner_points_ = nh.advertise<sensor_msgs::PointCloud2>("/past/corner_points", 2);
  pub_past_full_points_ = nh.advertise<sensor_msgs::PointCloud2>("/past/full_points", 2);

  pub_map_surf_points_ = nh.advertise<sensor_msgs::PointCloud2>("/map/surf_points", 2);
  pub_map_corner_points_ = nh.advertise<sensor_msgs::PointCloud2>("/map/corner_points", 2);

  pub_predict_surf_points_ = nh.advertise<sensor_msgs::PointCloud2>("/predict/surf_points", 2);
  pub_predict_corner_points_ = nh.advertise<sensor_msgs::PointCloud2>("/predict/corner_points", 2);
  pub_predict_full_points_ = nh.advertise<sensor_msgs::PointCloud2>("/predict/full_points", 2);

  pub_newest_surf_points_ = nh.advertise<sensor_msgs::PointCloud2>("/newest/surf_points", 2);
  pub_newest_corner_points_ = nh.advertise<sensor_msgs::PointCloud2>("/newest/corner_points", 2);
  pub_newest_full_points_ = nh.advertise<sensor_msgs::PointCloud2>("/newest/full_points", 2);

  pub_extrinsic_ = nh.advertise<geometry_msgs::PoseStamped>("/extrinsic_lb", 10);

  splineVisual = std::make_shared<splineVisualizer>("imu_init", nh);
}

void Estimator::ProcessImu(double dt,
                           const Vector3d &linear_acceleration,
                           const Vector3d &angular_velocity) {
    
    Eigen::Vector3d &bas = traj_manager_->calib_param_manager->acce_bias;
    Eigen::Vector3d &bgs = traj_manager_->calib_param_manager->gyro_bias;
    g_vec_ = traj_manager_->calib_param_manager->gravity;

    Vector3d un_acc_0 = imu_data_last_.q * (imu_data_last_.a - bas) + g_vec_;
    Vector3d un_gyr = 0.5 * (imu_data_last_.w + angular_velocity) - bgs;
    imu_data_last_.q *= DeltaQ(un_gyr * dt);
    imu_data_last_.q.normalize();
    Vector3d un_acc_1 = imu_data_last_.q * (linear_acceleration - bas) + g_vec_;
    Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
    imu_data_last_.p += dt * imu_data_last_.v + 0.5 * dt * dt * un_acc;
    imu_data_last_.v += dt * un_acc;

    imu_data_last_.a = linear_acceleration;
    imu_data_last_.w = angular_velocity;

}

void Estimator::DeskewOneCloud(const PointCloudPtr &pi, double te) {
  // 获取 te 对应的样条位姿 T_{te}^{I_0}
  Eigen::Vector3d p_Le_I0, p_Li_I0, p_Li_Le, point;
  Eigen::Quaterniond q_Le_I0, q_Li_I0, q_Li_Le;
  traj_manager_->evaluateLidarPose(te, p_Le_I0, q_Le_I0);

  for (int i = 0; i < pi->points.size(); ++i) {
    // 获取 ti 对应的位姿 T_{ti}^{I_0}
    double time = te - 0.1 + pi->points[i].intensity - int(pi->points[i].intensity);
    traj_manager_->evaluateLidarPose(time, p_Li_I0, q_Li_I0);
    point.x() = pi->points[i].x;
    point.y() = pi->points[i].y;
    point.z() = pi->points[i].z;
    // T_{ti}^{te} = T_{te}^{I_0}.inverse * T_{ti}^{I_0}
    // R_{ti}^{te} = R_{te}^{I_0}.inverse * R_{ti}^{I_0}
    // p_{ti}^{te} = R_{te}^{I_0}.inverse * (p_{ti}^{I_0} - p_{te}^{I_0})
    q_Li_Le = q_Le_I0.conjugate() * q_Li_I0;
    p_Li_Le = q_Le_I0.conjugate() * (p_Li_I0 - p_Le_I0);

    // pi = R_{ti}^{te} * pi + p_{ti}^{te}
    point = q_Li_Le * point + p_Li_Le;
    pi->points[i].x = point.x();
    pi->points[i].y = point.y();
    pi->points[i].z = point.z();
    // pi->points[i].intensity = 0;
  }
}

void Estimator::DeskewOneCloud(const PointCloudPtr &pi, PointCloudPtr &po, double te) {
  // 获取 te 对应的样条位姿 T_{te}^{I_0}
  Eigen::Vector3d p_Le_I0, p_Li_I0, p_Li_Le, point;
  Eigen::Quaterniond q_Le_I0, q_Li_I0, q_Li_Le;
  traj_manager_->evaluateLidarPose(te, p_Le_I0, q_Le_I0);
  po->points.resize(pi->points.size());
  
  for (int i = 0; i < pi->points.size(); ++i) {
    // 获取 ti 对应的位姿 T_{ti}^{I_0}
    double time = te - 0.1 + pi->points[i].intensity - int(pi->points[i].intensity);
    traj_manager_->evaluateLidarPose(time, p_Li_I0, q_Li_I0);
    point.x() = pi->points[i].x;
    point.y() = pi->points[i].y;
    point.z() = pi->points[i].z;
    // T_{ti}^{te} = T_{te}^{I_0}.inverse * T_{ti}^{I_0}
    // R_{ti}^{te} = R_{te}^{I_0}.inverse * R_{ti}^{I_0}
    // p_{ti}^{te} = R_{te}^{I_0}.inverse * (p_{ti}^{I_0} - p_{te}^{I_0})
    q_Li_Le = q_Le_I0.conjugate() * q_Li_I0;
    p_Li_Le = q_Le_I0.conjugate() * (p_Li_I0 - p_Le_I0);

    // pi = R_{ti}^{te} * pi + p_{ti}^{te}
    point = q_Li_Le * point + p_Li_Le;
    po->points[i].x = point.x();
    po->points[i].y = point.y();
    po->points[i].z = point.z();
    po->points[i].intensity = pi->points[i].intensity;
  }
}

PointCloudPtr Estimator::transformPointCloud(PointCloudPtr &cloudIn, double time) {

  PointCloudPtr cloudOut(new pcl::PointCloud<PointT>());

  PointT *pointFrom;
  PointT pointTo;

  int cloudSize = cloudIn->points.size();
  cloudOut->resize(cloudSize);

  Eigen::Vector3d p_Le_I0, point;
  Eigen::Quaterniond q_Le_I0;
  traj_manager_->evaluateLidarPose(time, p_Le_I0, q_Le_I0);

  for (int i = 0; i < cloudSize; ++i){

    pointFrom = &cloudIn->points[i];
    point.x() = pointFrom->x;
    point.y() = pointFrom->y;
    point.z() = pointFrom->z;
    point = q_Le_I0 * point + p_Le_I0;

    pointTo.x = point.x();
    pointTo.y = point.y();
    pointTo.z = point.z();
    pointTo.intensity = 0; 

    cloudOut->points[i] = pointTo;
  }

  return cloudOut;
}

void Estimator::PointAssociateToMap(const PointT &pi, PointT &po, double time) {
  Eigen::Vector3d p_Li_I0, point;
  Eigen::Quaterniond q_Li_I0;
  traj_manager_->evaluateLidarPose(time, p_Li_I0, q_Li_I0);
  point.x() = pi.x;
  point.y() = pi.y;
  point.z() = pi.z;
  point = q_Li_I0 * point + p_Li_I0;

  po.x = point.x();
  po.y = point.y();
  po.z = point.z();
  po.intensity = 0; 
}

void Estimator::ProcessCompactData(const sensor_msgs::PointCloud2ConstPtr &compact_data,
                                   const std_msgs::Header &header) {
  // 获取角点、面点、全
  TicToc tictoc;
  tictoc.Tic();
  PointMapping::CompactDataHandler(compact_data);

  // 将新来的点云存进 skew 队列中
  laser_cloud_surf_stack_downsampled_->clear();
  down_size_filter_surf_.setInputCloud(laser_cloud_surf_last_);
  down_size_filter_surf_.filter(*laser_cloud_surf_stack_downsampled_);

  laser_cloud_corner_stack_downsampled_->clear();
  down_size_filter_corner_.setInputCloud(laser_cloud_corner_last_);
  down_size_filter_corner_.filter(*laser_cloud_corner_stack_downsampled_);

  skew_surfs_.push_back(pcl::make_shared<pcl::PointCloud<PointT>>(*laser_cloud_surf_stack_downsampled_));
  skew_corners_.push_back(pcl::make_shared<pcl::PointCloud<PointT>>(*laser_cloud_corner_stack_downsampled_));
  skew_fulls_.push_back(pcl::make_shared<pcl::PointCloud<PointT>>(*full_cloud_));
  skew_timestamps_.push_back(header.stamp.toSec());  // 目前采用点云扫描结尾的时间 2021/11/19
  LOG(INFO) << "skew_surfs_ points size : " << skew_surfs_.back()->points.size();
  LOG(INFO) << "skew_corners_ points size : " << skew_corners_.back()->points.size();
  LOG(INFO) << "skew_fulls_ points size : " << skew_fulls_.back()->points.size();

  // 更新 segAttribute
  double lidar_end = skew_timestamps_.back();
  double lidar_start = lidar_end - 0.1;
  traj_manager_->updateSegments(lidar_start, lidar_end, estimator_config_.pre_knot_size);
  // 更新控制点的时候会固定原有的active控制点，因此初始化新控制点时，原有控制点不会改变，初始化后，会解除固定
  // 初始化控制点后，只优化active对应的控制点，也就是点云对应的控制点
  LOG(INFO) << "get new point cloud and update spline : " << tictoc.Toc() << " ms";
  // 初始化控制点
  if (!system_init_) {
    // 整个系统的初始化
    tictoc.Tic();
    if (traj_manager_->staticSeg.fid >= 0) {

      // 第一帧所有 imu 加速度的平均值作为初始的重力加速度
      int acc_count = 0;
      Eigen::Vector3d acc0(0., 0., 0.);
      for (auto &a : linear_acceleration_buf_[0]) {
        acc0 += a;
        acc_count++;
      }
      acc0 /= acc_count;
      // acc0 = linear_acceleration_buf_[0].front();
      Eigen::Vector3d unit_z(0, 0, 1);
      // IMU frame to inertial frame
      // acc0 = Eigen::Vector3d(0.8505, 1.24, -9.688);
      Eigen::Quaterniond q_wI0;
      if (estimator_config_.use_first_imu) {
        q_wI0 = first_imu_q;
      } else {
        q_wI0 = Eigen::Quaterniond::FromTwoVectors(acc0, unit_z);
      }

      Eigen::Vector3d WyrpI0 = q_wI0.toRotationMatrix().eulerAngles(2, 0, 1);
      q_wI0 = Eigen::Quaterniond(Eigen::AngleAxisd(-WyrpI0.x(), Eigen::Vector3d(0, 0, 1)).toRotationMatrix()) * q_wI0;
      WyrpI0 = q_wI0.toRotationMatrix().eulerAngles(2, 0, 1);
      LOG(INFO) << "world y r p I0: y: " << WyrpI0.x() << "  r: " << WyrpI0.y() << "  p: " << WyrpI0.z();
      // Eigen::Matrix3d R_wI0 = q_wI0.toRotationMatrix();
      // double yaw = R2ypr(R_wI0).x();
      // R_wI0 = ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R_wI0;

      Eigen::Matrix3d R_I0w = q_wI0.normalized().toRotationMatrix().transpose();
      Eigen::Vector3d g_init = R_I0w * Eigen::Vector3d(0, 0, -1) * kontiki::sensors::internal::STANDARD_GRAVITY;
      Eigen::Vector3d pryFromI0ToWorld = R_I0w.eulerAngles(1, 0, 2);
      LOG(INFO) << "init gravity : " << g_init.transpose();
      LOG(INFO) << "p r y from I0 to world, pitch: " << pryFromI0ToWorld.x() << "  roll: " << pryFromI0ToWorld.y() << "  yaw: " << pryFromI0ToWorld.z();

      traj_manager_->calib_param_manager->set_gravity(g_init);
      Twist<double> transform_bl = transform_lb_.cast<double>().inverse();
      traj_manager_->calib_param_manager->set_p_LinI(transform_bl.pos);
      traj_manager_->calib_param_manager->set_q_LtoI(transform_bl.rot);
      traj_manager_->calib_param_manager->set_gravity_orientation_pitch(pryFromI0ToWorld.x());
      traj_manager_->calib_param_manager->set_gravity_orientation_roll(pryFromI0ToWorld.y());

      auto &imu_I0 = origin_imu_buf_.front();
      imu_data_last_.a = linear_acceleration_buf_[0][0];
      imu_data_last_.w = angular_velocity_buf_[0][0];
      imu_data_last_.time = imu_timestamp_buf_[0][0];
      imu_data_last_.p = Eigen::Vector3d(0., 0., 0.);
      imu_data_last_.v = Eigen::Vector3d(0., 0., 0.);
      imu_data_last_.q = Eigen::Quaterniond(1, 0, 0, 0);

      // 计算当前全部的imu数据积分出来的p q v
      std::vector<imuPoseAndVel> initpqv;
      std::vector<Eigen::Vector3d> initgrys, initaccs;
      for (int i = 0; i < linear_acceleration_buf_.size(); ++i) {
        std::vector<Eigen::Vector3d> &acci = linear_acceleration_buf_[i];
        std::vector<Eigen::Vector3d> &gryi = angular_velocity_buf_[i];
        std::vector<double> &timei = imu_timestamp_buf_[i];

        for (int j = 0; j < acci.size(); ++j) {
          double dt = timei[j] - imu_data_last_.time;
          ProcessImu(dt, acci[j], gryi[j]);
          imu_data_last_.time = timei[j];
          initpqv.emplace_back();
          initpqv.back().time = imu_data_last_.time;
          initpqv.back().pos = imu_data_last_.p;
          initpqv.back().rot = imu_data_last_.q;
          initpqv.back().vel = imu_data_last_.v;

          initgrys.emplace_back();
          initgrys.back() = gryi[j];

          initaccs.emplace_back();
          initaccs.back() = acci[j];
        }
      }

      // for (int i = 0; i < initpqv.size(); i += 10) {
      //   ros::Time rt = ros::Time().fromSec(initpqv[i].time);
      //   pubOdometry(pub_discrete_imu_odom_, initpqv[i].pos, initpqv[i].rot, rt, discrete_imu_odom_);
      // }

      LOG(INFO) << "get init p q v : " << tictoc.Toc() << " ms";
      LOG(INFO) << "init q^w_I0 ypr = " << initpqv[0].rot.toRotationMatrix().eulerAngles(2, 1, 0).transpose();
      // 将积分值作为测量值带入到样条中优化
      // for (auto &m : initpqv) {
      //   std::cout << "p : " << m.pos.transpose() << std::endl;
      //   std::cout << "q : " << m.rot.coeffs().transpose() << std::endl;
      //   std::cout << "v : " << m.vel.transpose() << std::endl;
      //   std::cout << "\n";
      // }
      // 第一个rot值如果是 1 0 0 0，求导数会出现 nan，暂时不清楚什么原因 2021/11/22
      Eigen::AngleAxisd rotation_vector(0.000001, Eigen::Vector3d(0,0,1));
      Eigen::Quaterniond q0 = Eigen::Quaterniond (rotation_vector.matrix());
      initpqv[0].rot *= q0;

      traj_manager_->splineSystemInit(initpqv, initgrys, initaccs);
      Eigen::Vector3d pos_t0;
      Eigen::Quaterniond rot_t0;
      traj_manager_->evaluateImuPose(traj_manager_->getTrajectory()->R3Spline()->t0(), pos_t0, rot_t0);
      LOG(INFO) << "after spline system init, in t0, pos: " << pos_t0.transpose() << "  rot ypr: " << rot_t0.toRotationMatrix().eulerAngles(2, 1, 0).transpose();
      // pubNewestControlPoint();
      // for (int i = 0; i < initpqv.size(); i += 10) {
      //   ros::Time rt = ros::Time().fromSec(initpqv[i].time);
      //   Eigen::Vector3d p_Li_I0;
      //   Eigen::Quaterniond q_Li_I0;
      //   traj_manager_->evaluateLidarPose(initpqv[i].time, p_Li_I0, q_Li_I0);
      //   pubOdometry(pub_predict_odom_, p_Li_I0, q_Li_I0, rt, predict_odom_);
      // }
      LOG(INFO) << "splineSystemInit : " << tictoc.Toc() << " ms";
      // TODO: 显示初始化之后的轨迹?
      // ofstream outfile;
      // // 测量数据
      // outfile.open("/home/dut-zxw/zxw/catkin_LIO_Kontiki/initdata.txt");
      // for (int i = 0; i < initpqv.size(); ++i) {
      //     // Eigen::Vector3d p(0, i * 0.3, 0);
      //     auto p = initpqv[i].pos;
      //     auto q = initpqv[i].rot;
      //     auto v = initpqv[i].vel;
      //     outfile << p.x() << " " << p.y() << " " << p.z() << " "
      //     << q.x() << " " <<  q.y() << " " << q.z() << " " << q.w() << " " << initpqv[i].time - initpqv[0].time 
      //     << " " << v.x() << " " << v.y() << " " << v.z() << std::endl;
      // }
      // outfile.close();
      
      // 更新imu相关参数，包括gravity， roll、pitch
      traj_manager_->SetCalibParam();
      traj_manager_->ShowCalibParam();
      traj_manager_->initControlPointToGlobal();
      LOG(INFO) << "gyro_weight: " << traj_manager_->calib_param_manager->global_opt_gyro_weight;
      LOG(INFO) << "acce_weight: " << traj_manager_->calib_param_manager->global_opt_acce_weight;
      LOG(INFO) << "lidar_weight: " << traj_manager_->calib_param_manager->global_opt_lidar_weight;

      // 将第一帧 skew 点云存到 de-skew 以及 缓冲队列 中
      past_surfs_.push_back(skew_surfs_.front());
      buffer_surfs_.push_back(skew_surfs_.front());
      skew_surfs_.pop_front();

      past_corners_.push_back(skew_corners_.front());
      buffer_corners_.push_back(skew_corners_.front());
      skew_corners_.pop_front();

      buffer_fulls_.push_back(skew_fulls_.front());
      skew_fulls_.pop_front();

      past_timestamps_.push_back(skew_timestamps_.front());
      buffer_timestamps_.push_back(skew_timestamps_.front());
      skew_timestamps_.pop_front();

      system_init_ = true;
      LOG(INFO) << "initialization : " << tictoc.Toc() << " ms";
    } else {
      LOG(INFO) << "data not enough to init\n" << "static.fid : " << traj_manager_->staticSeg.fid;
    }

  } else {
    tictoc.Tic();
    std::vector<imuPoseAndVel> cur_pqv;
    std::vector<Eigen::Vector3d> cur_grys, cur_accs;
    std::vector<Eigen::Vector3d> &acci = linear_acceleration_buf_.last();
    std::vector<Eigen::Vector3d> &gryi = angular_velocity_buf_.last();
    std::vector<double> &timei = imu_timestamp_buf_.last();

    // 获取之前优化后的最后imu时刻的数据
    traj_manager_->evaluateImuPose(imu_data_last_.time, imu_data_last_.p, imu_data_last_.q);
    traj_manager_->evaluateImuVelocity(imu_data_last_.time, imu_data_last_.v);
    // imu_data_last_.v = Eigen::Vector3d(0.2, 0.2, 0.2);
    // traj_manager_->eavluateImuAcceleration(imu_data_last_.time, imu_data_last_.a);
    // traj_manager_->eavluateImuGryscope(imu_data_last_.time, imu_data_last_.w);


    double imu_start = traj_manager_->staticSeg.t0;
    double imu_end = traj_manager_->activeSeg.t0 + traj_manager_->activeSeg.n * traj_manager_->activeSeg.dt;
    LOG(INFO) << "imu_start time : " << imu_start - traj_manager_->getTrajectory()->R3Spline()->t0();
    LOG(INFO) << "imu_end time : " << imu_end - traj_manager_->getTrajectory()->R3Spline()->t0();
    LOG(INFO) << "spline_end time : " << traj_manager_->getTrajectory()->R3Spline()->MaxTime() - traj_manager_->getTrajectory()->R3Spline()->t0();
    // 使用当前帧的imu数据积分出 p q v
    LOG(INFO) << "timei.size : " << timei.size() << "  acci.size : " << acci.size() << "  gryi.size : " << gryi.size();
    if (timei.size() != acci.size() || timei.size() != gryi.size()) {
      LOG(WARNING) << "data size not match";
      return;
    }

    for (int i = 0; i < acci.size(); ++i) {
      // LOG(INFO) << "imu time : " << timei[i] - traj_manager_->getTrajectory()->R3Spline()->t0();
      if (timei[i] < imu_start || timei[i] > imu_end) {
        LOG(INFO) << "imu data time not match";
        continue;
      }

      double dt = timei[i] - imu_data_last_.time;
      if (dt < 0) {
        LOG(WARNING) << "dt < 0";
        continue;
      }
      ProcessImu(dt, acci[i], gryi[i]);
      imu_data_last_.time = timei[i];

      cur_pqv.emplace_back();
      cur_pqv.back().time = imu_data_last_.time;
      cur_pqv.back().pos = imu_data_last_.p;
      cur_pqv.back().rot = imu_data_last_.q;
      cur_pqv.back().vel = imu_data_last_.v;
      DLOG(INFO) << "cur_v[" << i << "] : " << imu_data_last_.v.transpose() << " a: " << acci[i].transpose() << endl;
      DLOG(INFO) << "cur_q[" << i << "] : " << imu_data_last_.q.toRotationMatrix().eulerAngles(2, 1, 0).transpose() << " w: " << gryi[i].transpose() << endl;
      DLOG(INFO) << "cur_p[" << i << "] : " << imu_data_last_.p.transpose() << endl;

      cur_grys.emplace_back();
      cur_grys.back() = gryi[i];

      cur_accs.emplace_back();
      cur_accs.back() = acci[i];
    }

    for (int i = cur_pqv.size() - 1; i >= 0; --i) {
      if (cur_pqv[i].time < skew_timestamps_.back()) {
        Twist<double> trans;
        
        trans.pos = cur_pqv[i].pos;
        trans.rot = cur_pqv[i].rot;
        lidar_pose_cur = trans.cast<float>();
        break;
      }
    }

    // ros::Rate rpre(10);
    // for (int i = 0; i < cur_pqv.size(); i += 3) {
    //   ros::Time rt = ros::Time().fromSec(cur_pqv[i].time);
    //   pubOdometry(pub_discrete_imu_odom_, cur_pqv[i].pos, cur_pqv[i].rot, rt, discrete_imu_odom_);
    //   rpre.sleep();
    // }
    std::vector<Eigen::Vector3d> discrete_pos;
    for (auto &m : cur_pqv) {
      discrete_pos.push_back(m.pos);
    }
    splineVisual->pubDiscretePath(discrete_pos);
    LOG(INFO) << "get init p q v : " << tictoc.Toc() << " ms";
    // 初始化新来控制点
    // pubAllControlPoint();
    traj_manager_->initNewControlPoint(cur_pqv, cur_grys, cur_accs);
    // pubNewestControlPoint();

    TicToc show_time_cost;
    show_time_cost.Tic();
    splineVisual->showSplinePath();
    splineVisual->showSplineControl();
    // std::getchar();
    LOG(INFO) << "draw path time : " << show_time_cost.Toc() << " ms";

    // for (int i = 0; i < cur_pqv.size(); i += 3) {
    //   ros::Time rt = ros::Time().fromSec(cur_pqv[i].time);
    //   Eigen::Vector3d p_Li_I0;
    //   Eigen::Quaterniond q_Li_I0;
    //   traj_manager_->evaluateLidarPose(cur_pqv[i].time, p_Li_I0, q_Li_I0);
    //   pubOdometry(pub_predict_odom_, p_Li_I0, q_Li_I0, rt, predict_odom_);
    //   rpre.sleep();
    // }
    LOG(INFO) << "initNewControlPoint : " << tictoc.Toc() << " ms";
    publishPredictCloud();

    // 将 skew 中时间小于 static.t0 的点云存到 de-skew 以及 缓冲队列 中
    LOG(INFO) << "skew_timestamps_.size : " << skew_timestamps_.size();
    LOG(INFO) << "traj_manager_->staticSeg.t0 : " << traj_manager_->staticSeg.t0 - traj_manager_->getTrajectory()->MinTime();
    LOG(INFO) << "skew_timestamps_.front() : " << skew_timestamps_.front() - traj_manager_->getTrajectory()->MinTime();
    while (!skew_timestamps_.empty() && skew_timestamps_.front() < traj_manager_->staticSeg.t0) {

      // {
      //   pcl::PCDWriter pcdw;
      //   std::string dir("/home/dut-zxw/zxw/data_bag/deskew/pcd/origin");
      //   static int pcdcount = 0;
      //   dir = dir + std::to_string(pcdcount) + ".pcd";
      //   pcdw.write(dir, *skew_fulls_.front());
      //   pcdcount++;
      // }
      // 对 skew.front 去畸变，transform to end
      DeskewOneCloud(skew_surfs_.front(), skew_timestamps_.front());
      DeskewOneCloud(skew_corners_.front(), skew_timestamps_.front());
      DeskewOneCloud(skew_fulls_.front(), skew_timestamps_.front());    

      buffer_surfs_.push_back(skew_surfs_.front());
      buffer_corners_.push_back(skew_corners_.front());
      buffer_fulls_.push_back(skew_fulls_.front());
      buffer_timestamps_.push_back(skew_timestamps_.front());

      // {
      //   pcl::PCDWriter pcdw;
      //   std::string dir("/home/dut-zxw/zxw/data_bag/deskew/pcd/bspline");
      //   static int spline_pcd_count = 0;
      //   dir = dir + std::to_string(spline_pcd_count) + ".pcd";
      //   pcdw.write(dir, *buffer_fulls_.back());
      //   spline_pcd_count++;
      // }

      {
        Eigen::Vector3d key_pos_last, key_pos_cur;
        Eigen::Quaterniond key_rot_last, key_rot_cur;
        traj_manager_->evaluateLidarPose(past_timestamps_.back(), key_pos_last, key_rot_last);
        traj_manager_->evaluateLidarPose(skew_timestamps_.front(), key_pos_cur, key_rot_cur);
        double angle_dist = key_rot_last.angularDistance(key_rot_cur);
        Eigen::Vector3d pos_dist = key_pos_last - key_pos_cur;
        double angle_threshold = 0.0 / 180.0 * 3.1415926;
        double pos_threshold = 0.0;
        if (std::abs(angle_dist) > angle_threshold || pos_dist.norm() > pos_threshold) {
          past_surfs_.push_back(skew_surfs_.front());
          past_corners_.push_back(skew_corners_.front());
          past_timestamps_.push_back(skew_timestamps_.front());
        }
      }

      skew_surfs_.pop_front();
      skew_corners_.pop_front();
      skew_fulls_.pop_front();
      skew_timestamps_.pop_front();

      LOG(INFO) << "push past, past size : " << past_timestamps_.size();
      LOG(INFO) << "buffer size : " << buffer_surfs_.size() << " " << buffer_corners_.size() 
      << " " << buffer_fulls_.size() << " " << buffer_timestamps_.size();
    }
    LOG(INFO) << "init control point : " << tictoc.Toc() << " ms";
    // TODO: 构造残差，添加到样条中优化
    // 1.初始化估计器
    // 2.添加残差
    // 3.优化求解
    extractSurroundingKeyFrames();
    LOG(INFO) << "extractSurroundingKeyFrames : " << tictoc.Toc() << " ms";
    int max_iteration = estimator_config_.iteration_size;
    for (int i = 0; i < max_iteration; ++i) {
      if (laserCloudCornerFromMapDS->points.size() < 10 || laserCloudSurfFromMapDS->points.size() < 100) {
        LOG(INFO) << "map points not enough";
        break;
      }
      LOG(INFO) << "surf map size : " << laserCloudSurfFromMapDS->points.size()
      << "  corner map size : " << laserCloudCornerFromMapDS->points.size();
      traj_manager_->trajInit();
      surfOptimization();
      LOG(INFO) << "add surf time: " << tictoc.Toc() << " ms";

      cornerOptimization();
      LOG(INFO) << "add corner time: : " << tictoc.Toc() << " ms";

      imuOptimization();
      // std::vector<Eigen::Vector3d> vel_meas;
      // std::vector<double> vel_times;
      // for (auto &m : cur_pqv) {
      //   // vel_meas.push_back(Eigen::Vector3d::Zero());
      //   vel_meas.push_back(m.vel);
      //   vel_times.push_back(m.time);
      // }
      // traj_manager_->addLinearVelocityMeasurement(vel_meas, vel_times);
      LOG(INFO) << "add imu time: " << tictoc.Toc() << " ms";
      
      bool is_convergence;
      traj_manager_->printErrorStatistics("before sovle", true, true, true);
      traj_manager_->solveOptProblem(is_convergence);
      traj_manager_->printErrorStatistics("after sovle", true, true, true);
      LOG(INFO) << "ceres solve time: " << tictoc.Toc() << " ms";

      // 更新imu与lidar的相关参数
      traj_manager_->SetCalibParam();
      traj_manager_->ShowCalibParam();
      // pubOptimizedControlPoint();
    }
    ba_last = traj_manager_->calib_param_manager->acce_bias;
    bg_last = traj_manager_->calib_param_manager->gyro_bias;
    LOG(INFO) << "the whole tight couple optimization : " << tictoc.Toc() << " ms";
    publishLocalMap();
    publishNewestCloud();
    // pubOneFrameDeskewPoints();
    show_time_cost.Tic();
    splineVisual->showSplinePath();
    splineVisual->showSplineControl();
    // std::getchar();
    LOG(INFO) << "draw path time : " << show_time_cost.Toc() << " ms";

    Eigen::Quaterniond C_w_I0 = traj_manager_->evaluateRotateAlignGravity();
    Eigen::Vector3d p_w_I0(0, 0, 0);
    double time = skew_timestamps_.back();
    ros::Time rt = ros::Time().fromSec(time);
    PublishTF(C_w_I0, p_w_I0, ros::Time().fromSec(skew_timestamps_.back()), tf_broadcaster_est_, "world", "imu_init");
  }
}

// 构建局部地图
void Estimator::extractSurroundingKeyFrames() {
  
  if (past_surfs_.empty() == true || past_corners_.empty() == true)
    return;

  if (recent_surfs_.size() < estimator_config_.surroundingKeyframeSearchNum) {
    recent_surfs_.clear();
    recent_corners_.clear();
    int numPoses = past_surfs_.size();
    last_recent_id = numPoses - 1;
    DLOG(INFO) << "past surf size : " << past_surfs_.size();
    DLOG(INFO) << "past timestamps size : " << past_timestamps_.size();
    for (int i = numPoses - 1; i >= 0; --i){
      double time = past_timestamps_[i];
      recent_surfs_.push_front(transformPointCloud(past_surfs_[i], time));
      recent_corners_.push_front(transformPointCloud(past_corners_[i], time));
      DLOG(INFO) << "push recent, recent size : " << recent_surfs_.size();
      if (recent_surfs_.size() >= estimator_config_.surroundingKeyframeSearchNum)
        break;
    }
  } else {
    while (last_recent_id + 1 < past_surfs_.size()) {
      last_recent_id++;
      recent_surfs_.pop_front();
      recent_corners_.pop_front();
      double time = past_timestamps_[last_recent_id];
      recent_surfs_.push_back(transformPointCloud(past_surfs_[last_recent_id], time));
      recent_corners_.push_back(transformPointCloud(past_corners_[last_recent_id], time));
    }
  }

  laserCloudSurfFromMap->clear();
  laserCloudCornerFromMap->clear();
  laserCloudSurfFromMapDS->clear();
  laserCloudCornerFromMapDS->clear();
  DLOG(INFO) << "clear";
  for (int i = 0; i < recent_surfs_.size(); ++i) {
    *laserCloudSurfFromMap   += *recent_surfs_[i];
    *laserCloudCornerFromMap += *recent_corners_[i];
  }
  DLOG(INFO) << "accumulate recent";

  down_size_filter_surf_.setInputCloud(laserCloudSurfFromMap);
  down_size_filter_surf_.filter(*laserCloudSurfFromMapDS);
  kdtree_surf_from_map->setInputCloud(laserCloudSurfFromMapDS);
  DLOG(INFO) << "surf kdtree input";
  down_size_filter_corner_.setInputCloud(laserCloudCornerFromMap);
  down_size_filter_corner_.filter(*laserCloudCornerFromMapDS);
  kdtree_corner_from_map->setInputCloud(laserCloudCornerFromMapDS);
}

// 角点残差
void Estimator::cornerOptimization() {
  PointT point_sel, point_ori, point_proj, coeff1, coeff2;

  std::vector<int> point_search_idx(5, 0);
  std::vector<float> point_search_sq_dis(5, 0);
  Eigen::Matrix<float, 5, 3> mat_A0;
  Eigen::Matrix<float, 5, 1> mat_B0;
  Eigen::Vector3f mat_X0;
  Eigen::Matrix3f mat_A1;
  Eigen::Matrix<float, 1, 3> mat_D1;
  Eigen::Matrix3f mat_V1;

  mat_A0.setZero();
  mat_B0.setConstant(-1);
  mat_X0.setZero();

  mat_A1.setZero();
  mat_D1.setZero();
  mat_V1.setZero();

  double active_t0 = traj_manager_->activeSeg.t0;
  LOG(INFO) << "static t0: " << active_t0 - traj_manager_->getTrajectory()->R3Spline()->t0();
  int count_f = 0;
  for (int frame = skew_timestamps_.size() - 1; frame >= 0; --frame) {
    if (skew_timestamps_[frame] - 0.1 < active_t0) {
      LOG(INFO) << "break lidar time: " << skew_timestamps_[frame] - traj_manager_->getTrajectory()->R3Spline()->t0();
      break;
    }
    count_f++;
    LOG(INFO) << "LIDAR frame " << count_f;

    const PointCloudPtr &origin_corner_points = skew_corners_[frame];
    size_t corner_points_size = origin_corner_points->points.size();
    const double cloud_time_end = skew_timestamps_[frame];

    //region Corner points
    for (int i = 0; i < corner_points_size; i++) {
      point_ori = origin_corner_points->points[i];
      double time = cloud_time_end - 0.1 + point_ori.intensity - int(point_ori.intensity);
      PointAssociateToMap(point_ori, point_sel, time);
      kdtree_corner_from_map->nearestKSearch(point_sel, 5, point_search_idx, point_search_sq_dis);

      if (point_search_sq_dis[4] < min_match_sq_dis_) {
        Eigen::Vector3f vc(0, 0, 0);

        for (int j = 0; j < 5; j++) {
          const PointT &point_sel_tmp = laserCloudCornerFromMapDS->points[point_search_idx[j]];
          vc.x() += point_sel_tmp.x;
          vc.y() += point_sel_tmp.y;
          vc.z() += point_sel_tmp.z;
        }
        vc /= 5.0;

        Eigen::Matrix3f mat_a;
        mat_a.setZero();

        for (int j = 0; j < 5; j++) {
          const PointT &point_sel_tmp = laserCloudCornerFromMapDS->points[point_search_idx[j]];
          Eigen::Vector3f a;
          a.x() = point_sel_tmp.x - vc.x();
          a.y() = point_sel_tmp.y - vc.y();
          a.z() = point_sel_tmp.z - vc.z();

          mat_a(0, 0) += a.x() * a.x();
          mat_a(1, 0) += a.x() * a.y();
          mat_a(2, 0) += a.x() * a.z();
          mat_a(1, 1) += a.y() * a.y();
          mat_a(2, 1) += a.y() * a.z();
          mat_a(2, 2) += a.z() * a.z();
        }
        mat_A1 = mat_a / 5.0;

        Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> esolver(mat_A1);
        mat_D1 = esolver.eigenvalues().real();
        mat_V1 = esolver.eigenvectors().real();

        if (mat_D1(0, 2) > 3 * mat_D1(0, 1)) {

          float x0 = point_sel.x;
          float y0 = point_sel.y;
          float z0 = point_sel.z;
          float x1 = vc.x() + 0.1 * mat_V1(0, 2);
          float y1 = vc.y() + 0.1 * mat_V1(1, 2);
          float z1 = vc.z() + 0.1 * mat_V1(2, 2);
          float x2 = vc.x() - 0.1 * mat_V1(0, 2);
          float y2 = vc.y() - 0.1 * mat_V1(1, 2);
          float z2 = vc.z() - 0.1 * mat_V1(2, 2);

          Eigen::Vector3f X0(x0, y0, z0);
          Eigen::Vector3f X1(x1, y1, z1);
          Eigen::Vector3f X2(x2, y2, z2);

          Eigen::Vector3f a012_vec = (X0 - X1).cross(X0 - X2);

          float a012 = a012_vec.norm();

          float l12 = (X1 - X2).norm();

          float ld2 = a012 / l12;

          float s = 1 - 0.9f * fabs(ld2);

          if (s > 0.1) {
            Eigen::Vector3d cur_point = Eigen::Vector3d{point_ori.x, point_ori.y, point_ori.z};
            Eigen::Vector3d last_a = X1.cast<double>();
            Eigen::Vector3d last_b = X2.cast<double>();
            // s = 1;
            traj_manager_->addEdgeMeasurement(cur_point, last_a, last_b, time, s);
          }
        }
      }
    }
  }
}

// 面点残差
void Estimator::surfOptimization() {
  PointT point_sel, point_ori, point_proj, coeff1, coeff2;

  std::vector<int> point_search_idx(5, 0);
  std::vector<float> point_search_sq_dis(5, 0);
  Eigen::Matrix<float, 5, 3> mat_A0;
  Eigen::Matrix<float, 5, 1> mat_B0;
  Eigen::Vector3f mat_X0;
  Eigen::Matrix3f mat_A1;
  Eigen::Matrix<float, 1, 3> mat_D1;
  Eigen::Matrix3f mat_V1;

  mat_A0.setZero();
  mat_B0.setConstant(-1);
  mat_X0.setZero();

  mat_A1.setZero();
  mat_D1.setZero();
  mat_V1.setZero();

  double active_t0 = traj_manager_->activeSeg.t0;
  LOG(INFO) << "static t0: " << active_t0 - traj_manager_->getTrajectory()->R3Spline()->t0();
  int count_f = 0;
  for (int frame = skew_timestamps_.size() - 1; frame >= 0; --frame) {
    if (skew_timestamps_[frame] - 0.1 < active_t0) {
      LOG(INFO) << "break lidar time: " << skew_timestamps_[frame] - traj_manager_->getTrajectory()->R3Spline()->t0();
      break;
    }
    count_f++;
    LOG(INFO) << "LIDAR frame " << count_f;

    const PointCloudPtr &origin_surf_points = skew_surfs_[frame];
    size_t surf_points_size = origin_surf_points->points.size();
    const double cloud_time_end = skew_timestamps_[frame];

    vector<unique_ptr<Feature>> features;

    // double delta = std::ceil(static_cast<double>(surf_points_size) / 3000.0);
    double delta = 1;
    std::cout << "delta = " << delta << std::endl;
    for (int i = 0; i < surf_points_size; i += delta) {
      point_ori = origin_surf_points->points[i];
      double time = cloud_time_end - 0.1 + point_ori.intensity - int(point_ori.intensity);
      PointAssociateToMap(point_ori, point_sel, time);

      int num_neighbors = 5;
      kdtree_surf_from_map->nearestKSearch(point_sel, num_neighbors, point_search_idx, point_search_sq_dis);

      if (point_search_sq_dis[num_neighbors - 1] < min_match_sq_dis_) {
        for (int j = 0; j < num_neighbors; j++) {
          mat_A0(j, 0) = laserCloudSurfFromMapDS->points[point_search_idx[j]].x;
          mat_A0(j, 1) = laserCloudSurfFromMapDS->points[point_search_idx[j]].y;
          mat_A0(j, 2) = laserCloudSurfFromMapDS->points[point_search_idx[j]].z;
        }
        mat_X0 = mat_A0.colPivHouseholderQr().solve(mat_B0);

        float pa = mat_X0(0, 0);
        float pb = mat_X0(1, 0);
        float pc = mat_X0(2, 0);
        float pd = 1;

        float ps = sqrt(pa * pa + pb * pb + pc * pc);
        pa /= ps;
        pb /= ps;
        pc /= ps;
        pd /= ps;

        // NOTE: plane as (x y z)*w+1 = 0

        bool planeValid = true;
        for (int j = 0; j < num_neighbors; j++) {
          if (fabs(pa * laserCloudSurfFromMapDS->points[point_search_idx[j]].x +
              pb * laserCloudSurfFromMapDS->points[point_search_idx[j]].y +
              pc * laserCloudSurfFromMapDS->points[point_search_idx[j]].z + pd) > min_plane_dis_) {
            planeValid = false;
            break;
          }
        }

        if (planeValid) {

          float pd2 = pa * point_sel.x + pb * point_sel.y + pc * point_sel.z + pd;

          float s = 1 - 0.9f * fabs(pd2) / sqrt(CalcPointDistance(point_sel));
          // s = 1.0;

          coeff1.x = s * pa;
          coeff1.y = s * pb;
          coeff1.z = s * pc;
          coeff1.intensity = s * pd;

          // coeff1.x = pa;
          // coeff1.y = pb;
          // coeff1.z = pc;
          // coeff1.intensity = pd;

          if (s > 0.1) {
            unique_ptr<PointPlaneFeature> feature = std::make_unique<PointPlaneFeature>();
            feature->score = time;
            feature->point = Eigen::Vector3d{point_ori.x, point_ori.y, point_ori.z};
            feature->coeffs = Eigen::Vector4d{coeff1.x, coeff1.y, coeff1.z, coeff1.intensity};
            features.push_back(std::move(feature));

            // Eigen::Vector3d cur_point = Eigen::Vector3d{point_ori.x, point_ori.y, point_ori.z};
            // Eigen::Vector3d plane_unit_norm = Eigen::Vector3d{coeff1.x, coeff1.y, coeff1.z};
            // double distance = coeff1.intensity;
            // traj_manager_->addPlaneMeasurement(cur_point, plane_unit_norm, distance, time);
          }
        }
      }
    }

    if (features.size() > 0) {
      for (int j = 0; j < features.size(); ++j) {
        PointPlaneFeature feature_j;
        features[j]->GetFeature(&feature_j);
        const double time = feature_j.score;

        Eigen::Vector3d cur_point = feature_j.point;
        const Eigen::Vector4d &coeff_eigen = feature_j.coeffs;

        Eigen::Vector3d plane_unit_norm = Eigen::Vector3d{coeff_eigen.x(), coeff_eigen.y(), coeff_eigen.z()};
        double distance = coeff_eigen.w();
        traj_manager_->addPlaneMeasurement(cur_point, plane_unit_norm, distance, time);
      }
    } else {
      LOG(INFO) << "surf features size: " << features.size() << " < 500, don't add surf feature";
    }

  }


  // Transform trans_ImToI0(imu_data_last_.q.cast<float>(), imu_data_last_.p.cast<float>());
  // Transform trans_LToI(traj_manager_->calib_param_manager->q_LtoI.cast<float>(), traj_manager_->calib_param_manager->p_LinI.cast<float>());
  // Transform trans_LmToI0 = trans_ImToI0 * trans_LToI;
  // vector<unique_ptr<Feature>> features;
  // CalculateLaserOdom(kdtree_surf_from_map, laserCloudSurfFromMapDS, origin_surf_points, trans_LmToI0, features);

  // for (int j = 0; j < features.size(); ++j) {
  //   PointPlaneFeature feature_j;
  //   features[j]->GetFeature(&feature_j);
  //   const double time = cloud_time_end - 0.1 + feature_j.score;

  //   Eigen::Vector3d cur_point = feature_j.point;
  //   const Eigen::Vector4d &coeff_eigen = feature_j.coeffs;

  //   Eigen::Vector3d plane_unit_norm = Eigen::Vector3d{coeff_eigen.x(), coeff_eigen.y(), coeff_eigen.z()};
  //   double distance = coeff_eigen.w();
  //   traj_manager_->addPlaneMeasurement(cur_point, plane_unit_norm, distance, time);
  // }
}

// imu测量值残差
void Estimator::imuOptimization() {
  double imu_start = traj_manager_->staticSeg.t0;
  double imu_end = traj_manager_->activeSeg.t0 + traj_manager_->activeSeg.n * traj_manager_->activeSeg.dt;
  std::vector<Eigen::Vector3d> grys, accs;
  std::vector<double> times;
  Eigen::Vector3d sp;
  double time_last = imu_timestamp_buf_.last().back() + 0.1;
  // 取 static 和 active segments 时间段上的 imu 数据
  for (int i = imu_timestamp_buf_.size() - 1; i >= 0; --i) {
    if (imu_timestamp_buf_[i].back() < imu_start) {
      break;
    }
    for (int j = imu_timestamp_buf_[i].size() - 1; j >= 0; --j) {
      if (imu_timestamp_buf_[i][j] < imu_start) {
        break;
      }
      if (time_last <= imu_timestamp_buf_[i][j]) {
        LOG(WARNING) << "time last <= time current";
      }
      time_last = imu_timestamp_buf_[i][j];
      times.push_back(imu_timestamp_buf_[i][j]);
      grys.push_back(angular_velocity_buf_[i][j]);
      accs.push_back(linear_acceleration_buf_[i][j]);
      // traj_manager_->eavluateImuGryscope(times.back(), sp);
      // LOG(INFO) << "measure gry: " << grys.back().transpose() << "  spline gry: " << sp.transpose() << " measure - spline: " << (grys.back() - sp).transpose();
      // traj_manager_->eavluateImuAcceleration(times.back(), sp);
      // LOG(INFO) << "measure acc: " << accs.back().transpose() << "  spline acc: " << sp.transpose() << " measure - spline: " << (accs.back() - sp).transpose();
    }
  }

  traj_manager_->addGyroscopeMeasurements(grys, times);
  traj_manager_->addAccelerometerMeasurement(accs, times);
  traj_manager_->addAcceAndGrysBiasMeasurement(ba_last, bg_last, estimator_config_.ba_weight, estimator_config_.bg_weight);
  // traj_manager_->addLinearVelocityMeasurement(accs, times);
}

void Estimator::ProcessEstimation() {

  while (true) {
    PairMeasurements measurements;
    std::unique_lock<std::mutex> buf_lk(buf_mutex_);
    con_.wait(buf_lk, [&] {
      return (measurements = GetMeasurements()).size() != 0;
    });
    buf_lk.unlock();

//    DLOG(INFO) << "measurement obtained: " << measurements.size() << ", first imu data size: "
//              << measurements.front().first.size();


    // 获取原始数据
    for (auto &measurement : measurements) {
      ROS_INFO_STREAM("measurements ratio: 1:" << measurement.first.size());
      // 由于第一帧点云对应的 IMU 数据一般来说是多于 0.1s的，因此为方便起见，直接不要第一帧数据了。
      if (!first_imu_) {
        first_imu_ = true;
        continue;
      }
      origin_compact_data_buf_.push(measurement.second);

      for (auto &imu_msg : measurement.first) {
        origin_imu_buf_.push(imu_msg);
      }
    }

    while (origin_compact_data_buf_.size() != 0) {
      if (origin_compact_data_buf_.front()->header.stamp.toSec() - lidar_time_last_ < estimator_config_.knot_distance) {
        LOG(INFO) << "lidar interval < " << estimator_config_.knot_distance;
        origin_compact_data_buf_.pop();
        continue;
      }
      CompactDataConstPtr compact_data_msg = origin_compact_data_buf_.front();
      // 初始化样条轨迹
      if (!built_traj_) {
        double start_time = origin_imu_buf_.front()->header.stamp.toSec();
        double end_time = compact_data_msg->header.stamp.toSec() + mm_config_.msg_time_delay;  // 点云的end时间，msg_time_delay表示end时间与时间戳的差值
        traj_manager_ = std::make_shared<TrajectoryManager>(start_time, end_time, estimator_config_.knot_distance);
        traj_manager_->calib_param_manager->global_opt_gyro_weight = estimator_config_.gry_weight;
        traj_manager_->calib_param_manager->global_opt_acce_weight = estimator_config_.acc_weight;
        traj_manager_->calib_param_manager->global_opt_lidar_weight = estimator_config_.lidar_weight;
        traj_manager_->calib_param_manager->gyro_bias = estimator_config_.gry_bias;
        traj_manager_->calib_param_manager->acce_bias = estimator_config_.acc_bias;
        bg_last = estimator_config_.gry_bias;
        ba_last = estimator_config_.acc_bias;
        LOG(INFO) << "ba weight: " << estimator_config_.ba_weight;
        LOG(INFO) << "bg weight: " << estimator_config_.bg_weight;
        // Eigen::Quaterniond q_wI0 = Eigen::Quaterniond(origin_imu_buf_.front()->orientation.w,
        //                                              origin_imu_buf_.front()->orientation.x,
        //                                              origin_imu_buf_.front()->orientation.y,
        //                                              origin_imu_buf_.front()->orientation.z);

        first_imu_q = Eigen::Quaterniond(origin_imu_buf_.front()->orientation.w,
                                         origin_imu_buf_.front()->orientation.x,
                                         origin_imu_buf_.front()->orientation.y,
                                         origin_imu_buf_.front()->orientation.z);

        splineVisual->bindTrajectory(traj_manager_);

        built_traj_ = true;
      }

      double lidar_end = compact_data_msg->header.stamp.toSec() + mm_config_.msg_time_delay;
      double lidar_start = lidar_end - 0.1;
      traj_manager_->extendTrajTo(lidar_start, lidar_end);

      double ax = 0, ay = 0, az = 0, rx = 0, ry = 0, rz = 0;
      bool has_full_knot = false;
      // 获取节点时间内的所有imu数据
      while (origin_imu_buf_.size() != 0) {
        auto &imu_msg = origin_imu_buf_.front();
        double imu_time = imu_msg->header.stamp.toSec();
        if (imu_time >= traj_manager_->getTrajectory()->R3Spline()->MaxTime()) {
          has_full_knot = true;
          break;
        }
        if (curr_time_ < 0) {
          curr_time_ = imu_time;
        }

        double dt = imu_time - curr_time_;
        ROS_ASSERT(dt >= 0);
        curr_time_ = imu_time;
        ax = imu_msg->linear_acceleration.x;
        ay = imu_msg->linear_acceleration.y;
        az = imu_msg->linear_acceleration.z;
        rx = imu_msg->angular_velocity.x;
        ry = imu_msg->angular_velocity.y;
        rz = imu_msg->angular_velocity.z;
        // 保存imu原始数据
        newKnotImu_acc.push_back(Vector3d(ax, ay, az));
        newKnotImu_gry.push_back(Vector3d(rx, ry, rz));
        newKnotImu_time.push_back(imu_time);
        origin_imu_buf_.pop();
      }

      if (has_full_knot) {
        LOG(INFO) << "new knot imu size : " << newKnotImu_time.size();
        linear_acceleration_buf_.push(newKnotImu_acc);
        angular_velocity_buf_.push(newKnotImu_gry);
        imu_timestamp_buf_.push(newKnotImu_time);
        newKnotImu_acc.clear();
        newKnotImu_gry.clear();
        newKnotImu_time.clear();
        // 此时的得到的数据为新增加的节点对应的所有imu数据，以及时间戳在该节点时间内的一帧imu数据
        TicToc t_s;
        thread_mutex_.lock();
        // this->ProcessCompactData(compact_data_msg, compact_data_msg->header);
        std_msgs::Header lidar_header = compact_data_msg->header;
        lidar_header.stamp = ros::Time().fromSec(lidar_end);
        this->ProcessCompactData(compact_data_msg, lidar_header);
        thread_mutex_.unlock();
        origin_compact_data_buf_.pop();
        lidar_time_last_ = compact_data_msg->header.stamp.toSec();

        saveAcceAndGrysMeasurements();
        
        double whole_t = t_s.Toc();
        LOG(INFO) << "whole time : " << whole_t << " ms";
      } else {
        break;
      }
    }
  }

}

// 发布一帧去畸变的点云
void Estimator::publishDeskewCloud() {
  static int skip_count = -1;
  skip_count++;
  if (skip_count % 2 != 0) {
    return;
  }
  std::string frame_id = "past";
  double time = buffer_timestamps_.front();
  Eigen::Vector3d p_Li_I0;
  Eigen::Quaterniond q_Li_I0;
  traj_manager_->evaluateLidarPose(time, p_Li_I0, q_Li_I0);
  ros::Time rt = ros::Time().fromSec(time);

  PublishCloudMsg(pub_past_surf_points_,
                  *buffer_surfs_.front(),
                  rt, frame_id);

  PublishCloudMsg(pub_past_corner_points_,
                  *buffer_corners_.front(),
                  rt, frame_id);
  
  PublishCloudMsg(pub_past_full_points_,
                  *buffer_fulls_.front(),
                  rt, frame_id);

  // PointCloudPtr tmp = transformPointCloud(buffer_fulls_.front(), time);
  // PublishCloudMsg(pub_past_full_points_,
  //                 *tmp,
  //                 rt, "imu_init");

  PublishTF(q_Li_I0, p_Li_I0, rt, tf_broadcaster_est_, "imu_init", frame_id);
  pubOdometry(pub_past_odom_, p_Li_I0, q_Li_I0, rt, past_odom_);
}

// 发布构建的局部地图
void Estimator::publishLocalMap() {
  std::string frame_id = "imu_init";
  double time = skew_timestamps_.back();
  ros::Time rt = ros::Time().fromSec(time);

  PublishCloudMsg(pub_local_surf_points_,
                  *laserCloudSurfFromMapDS,
                  rt, frame_id);

  PublishCloudMsg(pub_local_corner_points_,
                  *laserCloudCornerFromMapDS,
                  rt, frame_id);

}

// 发布全局地图
void Estimator::publishGlobalMap() {
  std::string frame_id = "imu_init";
  double time = past_timestamps_.back();
  ros::Time rt = ros::Time().fromSec(time);
  int past_nums = past_timestamps_.size();
  int delta = 10;

  PointCloudPtr global_map_surf = pcl::make_shared<PointCloud>();
  PointCloudPtr global_map_corner = pcl::make_shared<PointCloud>();
  for (int i = 0; i < past_nums; i += delta) {
    *global_map_surf += *transformPointCloud(past_surfs_[i], past_timestamps_[i]);
    *global_map_corner += *transformPointCloud(past_corners_[i], past_timestamps_[i]);
  }

  PointCloudPtr tmp = pcl::make_shared<PointCloud>();

  down_size_filter_surf_.setInputCloud(global_map_surf);
  down_size_filter_surf_.filter(*tmp);
  PublishCloudMsg(pub_map_surf_points_,
                  *tmp,
                  rt, frame_id);

  down_size_filter_corner_.setInputCloud(global_map_corner);
  down_size_filter_corner_.filter(*tmp);
  PublishCloudMsg(pub_map_corner_points_,
                  *tmp,
                  rt, frame_id);

}

// 发布根据imu数据初始化之后的预测点云
void Estimator::publishPredictCloud() {
  std::string frame_id = "predict";
  double time = skew_timestamps_.back();
  Eigen::Vector3d p_Li_I0;
  Eigen::Quaterniond q_Li_I0;
  traj_manager_->evaluateLidarPose(time, p_Li_I0, q_Li_I0);
  ros::Time rt = ros::Time().fromSec(time);

  PointCloudPtr tmp = pcl::make_shared<PointCloud>();

  DeskewOneCloud(skew_surfs_.back(), tmp, time);
  PublishCloudMsg(pub_predict_surf_points_,
                  *tmp,
                  rt, frame_id);

  DeskewOneCloud(skew_corners_.back(), tmp, time);
  PublishCloudMsg(pub_predict_corner_points_,
                  *tmp,
                  rt, frame_id);
  
  DeskewOneCloud(skew_fulls_.back(), tmp, time);
  PublishCloudMsg(pub_predict_full_points_,
                  *tmp,
                  rt, frame_id);

  PublishTF(q_Li_I0, p_Li_I0, rt, tf_broadcaster_est_, "imu_init", frame_id);
  pubOdometry(pub_predict_odom_, p_Li_I0, q_Li_I0, rt, predict_odom_);
}

// 发布最新的优化后的点云
void Estimator::publishNewestCloud() {
  std::string frame_id = "newest";
  double time = skew_timestamps_.back();
  Eigen::Vector3d p_Li_I0;
  Eigen::Quaterniond q_Li_I0;
  traj_manager_->evaluateLidarPose(time, p_Li_I0, q_Li_I0);
  ros::Time rt = ros::Time().fromSec(time);

  PointCloudPtr tmp = pcl::make_shared<PointCloud>();

  DeskewOneCloud(skew_surfs_.back(), tmp, time);
  PublishCloudMsg(pub_newest_surf_points_,
                  *tmp,
                  rt, frame_id);

  DeskewOneCloud(skew_corners_.back(), tmp, time);
  PublishCloudMsg(pub_newest_corner_points_,
                  *tmp,
                  rt, frame_id);
  
  DeskewOneCloud(skew_fulls_.back(), tmp, time);
  PublishCloudMsg(pub_newest_full_points_,
                  *tmp,
                  rt, frame_id);

  PublishTF(q_Li_I0, p_Li_I0, rt, tf_broadcaster_est_, "imu_init", frame_id);
  pubOdometry(pub_newest_odom_, p_Li_I0, q_Li_I0, rt, newest_odom_);
}

void Estimator::pubDeskewThread() {
  ros::Rate r(10);
  while (ros::ok()) {
    thread_mutex_.lock();

    if (buffer_timestamps_.empty()) {
      thread_mutex_.unlock();
      continue;
    }
    publishDeskewCloud();
    buffer_timestamps_.pop_front();
    buffer_surfs_.pop_front();
    buffer_corners_.pop_front();
    buffer_fulls_.pop_front();
    LOG(INFO) << "pop a buffer frame";

    thread_mutex_.unlock();
    r.sleep();
  }
}

void Estimator::pubOneFrameDeskewPoints() {
  if (buffer_timestamps_.empty()) {
    return;
  }
  publishDeskewCloud();
  buffer_timestamps_.pop_front();
  buffer_surfs_.pop_front();
  buffer_corners_.pop_front();
  buffer_fulls_.pop_front();
  LOG(INFO) << "one frame pop a buffer frame";
}

void Estimator::pubGlobalMapThread() {
  ros::Rate r(0.2);
  while (ros::ok()) {
    if (past_timestamps_.empty()) {
      continue;
    }
    publishGlobalMap();
    r.sleep();
  }
}

void Estimator::pubNewestControlPoint() {
  std::string frame_id = "newest_control";
  Eigen::Vector3d cp;
  Eigen::Quaterniond cq;
  ros::Rate r(100);
  for (int i = traj_manager_->getTrajectory()->R3Spline()->NumKnots() - 1, j = 0; i >= 0 && j < 10; --i, ++j) {
    cp = traj_manager_->getTrajectory()->R3Spline()->GetContralPointaAt(i);
    cq = traj_manager_->getTrajectory()->SO3Spline()->GetContralPointaAt(i);
    ros::Time rt = ros::Time().now();
    pubOdometry(pub_control_point_, cp, cq, rt, control_point_);
    PublishTF(cq, cp, rt, tf_broadcaster_est_, "imu_init", frame_id);
    r.sleep();
  }
}

void Estimator::pubOptimizedControlPoint() {
  std::string frame_id = "optimized_control";
  Eigen::Vector3d cp;
  Eigen::Quaterniond cq;
  ros::Rate r(100);
  for (int i = traj_manager_->getTrajectory()->R3Spline()->NumKnots() - 1, j = 0; i >= 0 && j < 10; --i, ++j) {
    cp = traj_manager_->getTrajectory()->R3Spline()->GetContralPointaAt(i);
    cq = traj_manager_->getTrajectory()->SO3Spline()->GetContralPointaAt(i);
    ros::Time rt = ros::Time().now();
    pubOdometry(pub_opt_cp_, cp, cq, rt, control_point_);
    PublishTF(cq, cp, rt, tf_broadcaster_est_, "imu_init", frame_id);
    r.sleep();
  }
}

void Estimator::CalculateLaserOdom(const pcl::KdTreeFLANN<PointT>::Ptr &kdtree_surf_from_map,
                                   const PointCloudPtr &local_surf_points_filtered_ptr,
                                   const PointCloudPtr &surf_stack,
                                   Transform &local_transform,
                                   vector<unique_ptr<Feature>> &features) {

  bool is_degenerate = false;
  for (size_t iter_count = 0; iter_count < num_max_iterations_; ++iter_count) {
    CalculateFeatures(kdtree_surf_from_map, local_surf_points_filtered_ptr, surf_stack,
                      local_transform, features);

    size_t laser_cloud_sel_size = features.size();
    Eigen::Matrix<float, Eigen::Dynamic, 6> mat_A(laser_cloud_sel_size, 6);
    Eigen::Matrix<float, 6, Eigen::Dynamic> mat_At(6, laser_cloud_sel_size);
    Eigen::Matrix<float, 6, 6> matAtA;
    Eigen::VectorXf mat_B(laser_cloud_sel_size);
    Eigen::VectorXf mat_AtB;
    Eigen::VectorXf mat_X;
    Eigen::Matrix<float, 6, 6> matP;

    PointT point_sel, point_ori, coeff;

    SO3 R_SO3(local_transform.rot); /// SO3

    for (int i = 0; i < laser_cloud_sel_size; i++) {
      PointPlaneFeature feature_i;
      features[i]->GetFeature(&feature_i);
      point_ori.x = feature_i.point.x();
      point_ori.y = feature_i.point.y();
      point_ori.z = feature_i.point.z();
      coeff.x = feature_i.coeffs.x();
      coeff.y = feature_i.coeffs.y();
      coeff.z = feature_i.coeffs.z();
      coeff.intensity = feature_i.coeffs.w();

      Eigen::Vector3f p(point_ori.x, point_ori.y, point_ori.z);
      Eigen::Vector3f w(coeff.x, coeff.y, coeff.z);

//      Eigen::Vector3f J_r = w.transpose() * RotationVectorJacobian(R_SO3, p);
      Eigen::Vector3f J_r = -w.transpose() * (local_transform.rot * SkewSymmetric(p));
      Eigen::Vector3f J_t = w.transpose();

      float d2 = w.transpose() * (local_transform.rot * p + local_transform.pos) + coeff.intensity;

      mat_A(i, 0) = J_r.x();
      mat_A(i, 1) = J_r.y();
      mat_A(i, 2) = J_r.z();
      mat_A(i, 3) = J_t.x();
      mat_A(i, 4) = J_t.y();
      mat_A(i, 5) = J_t.z();
      mat_B(i, 0) = -d2;
    }

    mat_At = mat_A.transpose();
    matAtA = mat_At * mat_A;
    mat_AtB = mat_At * mat_B;
    mat_X = matAtA.colPivHouseholderQr().solve(mat_AtB);

    if (iter_count == 0) {
      Eigen::Matrix<float, 1, 6> mat_E;
      Eigen::Matrix<float, 6, 6> mat_V;
      Eigen::Matrix<float, 6, 6> mat_V2;

      Eigen::SelfAdjointEigenSolver<Eigen::Matrix<float, 6, 6>> esolver(matAtA);
      mat_E = esolver.eigenvalues().real();
      mat_V = esolver.eigenvectors().real();

      mat_V2 = mat_V;

      is_degenerate = false;
      float eignThre[6] = {100, 100, 100, 100, 100, 100};
      for (int i = 0; i < 6; ++i) {
        if (mat_E(0, i) < eignThre[i]) {
          for (int j = 0; j < 6; ++j) {
            mat_V2(i, j) = 0;
          }
          is_degenerate = true;
          DLOG(WARNING) << "degenerate case";
          DLOG(INFO) << mat_E;
        } else {
          break;
        }
      }
      matP = mat_V2 * mat_V.inverse();
    }

    if (is_degenerate) {
      Eigen::Matrix<float, 6, 1> matX2(mat_X);
      mat_X = matP * matX2;
    }

    local_transform.pos.x() += mat_X(3, 0);
    local_transform.pos.y() += mat_X(4, 0);
    local_transform.pos.z() += mat_X(5, 0);

    local_transform.rot = local_transform.rot * DeltaQ(Eigen::Vector3f(mat_X(0, 0), mat_X(1, 0), mat_X(2, 0)));

    if (!isfinite(local_transform.pos.x())) local_transform.pos.x() = 0.0;
    if (!isfinite(local_transform.pos.y())) local_transform.pos.y() = 0.0;
    if (!isfinite(local_transform.pos.z())) local_transform.pos.z() = 0.0;

    float delta_r = RadToDeg(R_SO3.unit_quaternion().angularDistance(local_transform.rot));
    float delta_t = sqrt(pow(mat_X(3, 0) * 100, 2) + pow(mat_X(4, 0) * 100, 2) + pow(mat_X(5, 0) * 100, 2));

    if (delta_r < delta_r_abort_ && delta_t < delta_t_abort_) {
      DLOG(INFO) << "CalculateLaserOdom iter_count: " << iter_count;
      break;
    }
  }
}

void Estimator::CalculateFeatures(const pcl::KdTreeFLANN<PointT>::Ptr &kdtree_surf_from_map,
                                  const PointCloudPtr &local_surf_points_filtered_ptr,
                                  const PointCloudPtr &surf_stack,
                                  const Transform &local_transform,
                                  vector<unique_ptr<Feature>> &features) {

  PointT point_sel, point_ori, point_proj, coeff1, coeff2;
  if (!estimator_config_.keep_features) {
    features.clear();
  }

  std::vector<int> point_search_idx(5, 0);
  std::vector<float> point_search_sq_dis(5, 0);
  Eigen::Matrix<float, 5, 3> mat_A0;
  Eigen::Matrix<float, 5, 1> mat_B0;
  Eigen::Vector3f mat_X0;
  Eigen::Matrix3f mat_A1;
  Eigen::Matrix<float, 1, 3> mat_D1;
  Eigen::Matrix3f mat_V1;

  mat_A0.setZero();
  mat_B0.setConstant(-1);
  mat_X0.setZero();

  mat_A1.setZero();
  mat_D1.setZero();
  mat_V1.setZero();

  PointCloud laser_cloud_ori;
  PointCloud coeff_sel;
  vector<float> scores;

  const PointCloudPtr &origin_surf_points = surf_stack;
  const Transform &transform_to_local = local_transform;
  size_t surf_points_size = origin_surf_points->points.size();

  for (int i = 0; i < surf_points_size; i++) {
    point_ori = origin_surf_points->points[i];
    PointMapping::PointAssociateToMap(point_ori, point_sel, transform_to_local);

    int num_neighbors = 5;
    kdtree_surf_from_map->nearestKSearch(point_sel, num_neighbors, point_search_idx, point_search_sq_dis);

    if (point_search_sq_dis[num_neighbors - 1] < min_match_sq_dis_) {
      for (int j = 0; j < num_neighbors; j++) {
        mat_A0(j, 0) = local_surf_points_filtered_ptr->points[point_search_idx[j]].x;
        mat_A0(j, 1) = local_surf_points_filtered_ptr->points[point_search_idx[j]].y;
        mat_A0(j, 2) = local_surf_points_filtered_ptr->points[point_search_idx[j]].z;
      }
      mat_X0 = mat_A0.colPivHouseholderQr().solve(mat_B0);

      float pa = mat_X0(0, 0);
      float pb = mat_X0(1, 0);
      float pc = mat_X0(2, 0);
      float pd = 1;

      float ps = sqrt(pa * pa + pb * pb + pc * pc);
      pa /= ps;
      pb /= ps;
      pc /= ps;
      pd /= ps;

      // NOTE: plane as (x y z)*w+1 = 0

      bool planeValid = true;
      for (int j = 0; j < num_neighbors; j++) {
        if (fabs(pa * local_surf_points_filtered_ptr->points[point_search_idx[j]].x +
            pb * local_surf_points_filtered_ptr->points[point_search_idx[j]].y +
            pc * local_surf_points_filtered_ptr->points[point_search_idx[j]].z + pd) > min_plane_dis_) {
          planeValid = false;
          break;
        }
      }

      if (planeValid) {

        float pd2 = pa * point_sel.x + pb * point_sel.y + pc * point_sel.z + pd;

        float s = 1 - 0.9f * fabs(pd2) / sqrt(CalcPointDistance(point_sel));

        coeff1.x = s * pa;
        coeff1.y = s * pb;
        coeff1.z = s * pc;
        coeff1.intensity = s * pd;

        bool is_in_laser_fov = false;
        PointT transform_pos;
        PointT point_on_z_axis;

        point_on_z_axis.x = 0.0;
        point_on_z_axis.y = 0.0;
        point_on_z_axis.z = 10.0;
        PointMapping::PointAssociateToMap(point_on_z_axis, point_on_z_axis, transform_to_local);

        transform_pos.x = transform_to_local.pos.x();
        transform_pos.y = transform_to_local.pos.y();
        transform_pos.z = transform_to_local.pos.z();
        float squared_side1 = CalcSquaredDiff(transform_pos, point_sel);
        float squared_side2 = CalcSquaredDiff(point_on_z_axis, point_sel);

        float check1 = 100.0f + squared_side1 - squared_side2
            - 10.0f * sqrt(3.0f) * sqrt(squared_side1);

        float check2 = 100.0f + squared_side1 - squared_side2
            + 10.0f * sqrt(3.0f) * sqrt(squared_side1);

        if (check1 < 0 && check2 > 0) { /// within +-60 degree
          is_in_laser_fov = true;
        }

        if (s > 0.1 && is_in_laser_fov) {
          unique_ptr<PointPlaneFeature> feature = std::make_unique<PointPlaneFeature>();
          feature->score = point_ori.intensity - int(point_ori.intensity) ;
          feature->point = Eigen::Vector3d{point_ori.x, point_ori.y, point_ori.z};
          feature->coeffs = Eigen::Vector4d{coeff1.x, coeff1.y, coeff1.z, coeff1.intensity};
          features.push_back(std::move(feature));
        }
      }
    }
  }
}

void Estimator::saveAcceAndGrysMeasurements() {
  static int count = 0;
  std::string measure_dir = "/home/dut-zxw/zxw/catkin_LIO_Kontiki/data/measure_" + std::to_string(count) + ".txt";
  std::string spline_dir = "/home/dut-zxw/zxw/catkin_LIO_Kontiki/data/spline_"+ std::to_string(count) + ".txt";
  // count++;

  ofstream of;
  of.open(measure_dir, ios::app);
  // of << "measurement acceleration x y z, gryscorpe x y z, tiemstamp\n";
  for (int i = 0; i < linear_acceleration_buf_.last().size(); ++i) {
    Eigen::Vector3d &acc = linear_acceleration_buf_.last()[i];
    Eigen::Vector3d &gry = angular_velocity_buf_.last()[i];
    double time = imu_timestamp_buf_.last()[i] - traj_manager_->getTrajectory()->R3Spline()->t0();
    of.flags(ios::fixed);
    of.precision(5); //设置输出精度
    of << acc.x() << " " << acc.y() << " " << acc.z()
    << " " << gry.x() << " " << gry.y() << " " << gry.z() << " " << time << std::endl;
  }

  of.close();

  of.open(spline_dir, ios::app);
  // of << "spline acceleration x y z, gryscorpe x y z, tiemstamp\n";
  for (int i = 0; i < linear_acceleration_buf_.last().size(); ++i) {
    Eigen::Vector3d acc, gry;
    double unixTime = imu_timestamp_buf_.last()[i];
    traj_manager_->eavluateImuAcceleration(unixTime, acc);
    traj_manager_->eavluateImuGryscope(unixTime, gry);

    double time = unixTime - traj_manager_->getTrajectory()->R3Spline()->t0();
    of.flags(ios::fixed);
    of.precision(5); //设置输出精度
    of << acc.x() << " " << acc.y() << " " << acc.z()
    << " " << gry.x() << " " << gry.y() << " " << gry.z() << " " << time << std::endl;
  }

  of.close();

}


} // namespace lio